#include "solve.h"
#include "matrix.h"

Matrix jacrot(Matrix a, int i, int j)       //用于jiacobi方法的Givens变换
{
    Matrix b = ones(a.row);
    double y = (a.datap[(j-1)*(a.col+1)]-a.datap[(i-1)*(a.col+1)])/(2*a.datap[(i-1)*a.col+j-1]);
    double t;
    if (y>=0)
        t = sqrt(y * y + 1) - y;
    else
        t = -sqrt(y * y + 1) - y;
    double c = 1 / (sqrt(1 + t * t));
    double s = t * c;
    b.datap[(i - 1) * (b.col + 1)] = c;
    b.datap[(j - 1) * (b.col + 1)] = c;
    b.datap[(i - 1) * b.col + j-1] = s;
    b.datap[(j - 1) * b.col + i-1] = -s;
    return b;
}

void jaceig(Matrix a)       //过关jacobi方法求矩阵所有特征值和特征向量
{
    double eps = 0.001;
    int cnt = 0;
    Matrix q = ones(a.row);
    while (1)
    {
        int mark = cnt;
        for (int i = 1; i < a.row+1; i++)
        {
            for (int j = i+1; j < a.col + 1; j++)
            {
                if (fabs(a.datap[(i-1)*a.col+j-1])>eps)
                {
                    Matrix r = jacrot(a, i, j);
                    Matrix m = trans(r);
                    a = m * a;
                    a = a * r;
                    q = q * r;
                    cnt++;
                }
            }
        }
        eps /= 2;
        if (cnt==mark)
            break;
    }
    a.diag().show();
    q.show();
}

/*int main()
{
    double p[9] = {4, 1, 0, 1, 4, 1, 0, 1, 4};
    Matrix a(3, 3, p);
    jaceig(a);
}*/